import matplotlib.pyplot as plt
import numpy as np

# 读取数据
data = np.loadtxt('simulation_data.txt')

# 分离状态量和真值
steps = data[:, 0]
dx = data[:, 1]
dy = data[:, 2]
dz = data[:, 3]
dxdot = data[:, 4]
dydot = data[:, 5]
dzdot = data[:, 6]
true_dx = data[:, 7]
true_dy = data[:, 8]
true_dz = data[:, 9]
true_dxdot = data[:, 10]
true_dydot = data[:, 11]
true_dzdot = data[:, 12]

# 绘制dx
plt.figure(figsize=(14, 10))
plt.subplot(3, 2, 1)
plt.plot(steps, dx, label='Estimated dx')
plt.plot(steps, true_dx, label='True dx', linestyle='--')
plt.xlabel('Step')
plt.ylabel('dx')
plt.legend()

# 绘制dy
plt.subplot(3, 2, 2)
plt.plot(steps, dy, label='Estimated dy')
plt.plot(steps, true_dy, label='True dy', linestyle='--')
plt.xlabel('Step')
plt.ylabel('dy')
plt.legend()

# 绘制dz
plt.subplot(3, 2, 3)
plt.plot(steps, dz, label='Estimated dz')
plt.plot(steps, true_dz, label='True dz', linestyle='--')
plt.xlabel('Step')
plt.ylabel('dz')
plt.legend()

# 绘制dxdot
plt.subplot(3, 2, 4)
plt.plot(steps, dxdot, label='Estimated dxdot')
plt.plot(steps, true_dxdot, label='True dxdot', linestyle='--')
plt.xlabel('Step')
plt.ylabel('dxdot')
plt.legend()

# 绘制dydot
plt.subplot(3, 2, 5)
plt.plot(steps, dydot, label='Estimated dydot')
plt.plot(steps, true_dydot, label='True dydot', linestyle='--')
plt.xlabel('Step')
plt.ylabel('dydot')
plt.legend()

# 绘制dzdot
plt.subplot(3, 2, 6)
plt.plot(steps, dzdot, label='Estimated dzdot')
plt.plot(steps, true_dzdot, label='True dzdot', linestyle='--')
plt.xlabel('Step')
plt.ylabel('dzdot')
plt.legend()

plt.tight_layout()
plt.show()